# Image Comparison Tool version 1


import cv, time, sys
startTime = time.time()

def initiate(cmdLineArgs):
	fStartTime = time.time()
	print "Initializing..."
	# if len(cmdLineArgs)<1:
    # print "Sorry, you need to pass an argument.  Please pass in the path to the test image in quotations."
    # sys.exit(1)

	#arg1 = str(cmdLineArgs[1])
	# arg0 = "Images/reference1.JPG"
	# arg1 = "Images/test1.jpg"
	# arg2 = "Images/maskFace.JPG"
	# arg3 = "Images/maskMagenta.JPG"
	# arg4 = "Images/maskA.JPG"

	arg0 = "C:\\Documents and Settings\\Blair\\My Documents\\MishaPython\\HP\\Images\\reference1.JPG"
	arg1 = "C:\\Documents and Settings\\Blair\\My Documents\\MishaPython\\HP\\Images\\test1.jpg"
	arg2 = "C:\\Documents and Settings\\Blair\\My Documents\\MishaPython\\HP\\Images\\maskFace.jpg"
	arg3 = "C:\\Documents and Settings\\Blair\\My Documents\\MishaPython\\HP\\Images\\maskMagenta.jpg"
	arg4 = "C:\\Documents and Settings\\Blair\\My Documents\\MishaPython\\HP\\Images\\maskA.jpg"
	lastSlash = arg1.rfind('\\')
	imName = arg1[lastSlash+1:len(arg1)-4]
	imDir = arg1[0:lastSlash]
  
	referenceImage = cv.LoadImage(arg0)
	sourceImage = cv.LoadImage(arg1)
	grayReferenceImage = cv.LoadImage(arg0,cv.CV_LOAD_IMAGE_GRAYSCALE)
	graySourceImage = cv.LoadImage(arg1,cv.CV_LOAD_IMAGE_GRAYSCALE)
	maskFace = cv.LoadImage(arg2,cv.CV_LOAD_IMAGE_GRAYSCALE)
	maskMagenta = cv.LoadImage(arg3,cv.CV_LOAD_IMAGE_GRAYSCALE)
	maskA = cv.LoadImage(arg4,cv.CV_LOAD_IMAGE_GRAYSCALE)
  
	fEndTime = time.time()-fStartTime
	print "Images loaded in "+str(fEndTime)+" seconds"
	return (referenceImage,sourceImage,grayReferenceImage,graySourceImage,maskFace,maskMagenta,maskA,imName,imDir)
  
def resizeToSmallerImage(referenceImage,sourceImage):
	fStartTime = time.time()
	sRef = cv.GetSize(referenceImage)
	sSource = cv.GetSize(sourceImage)
	if (sRef[0]>sSource[0])|(sRef[1]>sSource[1]):
		print "Source image is smaller than reference image"
	else:
		print "Size of referenceImage is "+str(sRef)
		print "Size of inputImage is "+str(sSource)
		print "Source image will be downsampled to size of reference image"
		resizedSource = cv.CreateImage(sRef,cv.IPL_DEPTH_8U,3)
		cv.Resize(sourceImage,resizedSource)
		newSize = cv.GetSize(resizedSource)
		print "New Source image size is "+str(newSize)
	fEndTime = fStartTime-time.time()
	print "Image resized in "+str(fEndTime)+" seconds"
	return(resizedSource)
	
def resizeImage(inputImage):
	fStartTime = time.time()
	s = cv.GetSize(inputImage)
	s1 = (s[0],300)
	print "Size of input image is "+str(s)
	resizedImage = cv.CreateImage(s1,cv.IPL_DEPTH_8U,3)
	cv.Resize(inputImage,resizedImage)
	newSize = cv.GetSize(resizedImage)
	print "New image size is "+str(newSize)
	fEndTime = fStartTime-time.time()
	print "Image resized in "+str(fEndTime)+" seconds"
	return(resizedImage)
  
def drawMask():
  fStartTime = time.time()
  s = (100,100)
  center = (s[0]/2,s[1]/2)
  r1 = 10
  r2 = 20
  mask = cv.CreateImage(s,cv.IPL_DEPTH_8U,1)
  cv.Rectangle(mask,(0,0),s,[255,255,255],cv.CV_FILLED)
  cv.Circle(mask,center,r1,[0,0,0],-1)
  cv.Circle(mask,center,r2,[0,0,0],2)
  fEndTime = time.time()-fStartTime
  print "Mask drawn in "+str(fEndTime)+" seconds"
  return (mask)
  
def drawFiducials(inputImage,center1,center2,center3):
	center1 = (int(center1[0]),int(center1[1]))
	center2 = (int(center2[0]),int(center2[1]))
	center3 = (int(center3[0]),int(center3[1]))
	fStartTime = time.time()
	s = cv.GetSize(inputImage)
	inputImageWithFid = cv.CreateImage(s,cv.IPL_DEPTH_8U,1)
	cv.Copy(inputImage,inputImageWithFid)
	r1 = 5
	r2 = 10
	cv.Circle(inputImageWithFid,center1,r1,[0,0,0],-1)
	cv.Circle(inputImageWithFid,center1,r2,[0,0,0],2)
	cv.Circle(inputImageWithFid,center2,r1,[0,0,0],-1)
	cv.Circle(inputImageWithFid,center2,r2,[0,0,0],2)
	cv.Circle(inputImageWithFid,center3,r1,[0,0,0],-1)
	cv.Circle(inputImageWithFid,center3,r2,[0,0,0],2)
	fEndTime = time.time()-fStartTime
	print "Fiducials marked in "+str(fEndTime)+" seconds"
	return (inputImageWithFid)
  
def matchMask(inputImage,mask):
  fStartTime = time.time()
  imSize = cv.GetSize(inputImage)
  inputMat = cv.GetMat(inputImage)
  maskMat = cv.GetMat(mask)
  maskSize = cv.GetSize(maskMat)
  corrImage = cv.CreateMat(imSize[1]-maskSize[1]+1,imSize[0]-maskSize[0]+1,cv.CV_32FC1)
  corrImageSize = cv.GetSize(corrImage)
  cv.MatchTemplate(inputMat,maskMat,corrImage,cv.CV_TM_CCOEFF_NORMED)
  min,max,minLoc,maxLoc = cv.MinMaxLoc(corrImage)
  fEndTime = time.time()-fStartTime
  print "Fiducials detected in "+str(fEndTime)+" seconds"
  return(maxLoc)
  
def convertToGray(inputImage):
  fStartTime = time.time()
  s = cv.GetSize(inputImage)
  grayImage = cv.CreateImage(s,cv.IPL_DEPTH_8U,1)
  cv.CvtColor(inputImage,grayImage,cv.CV_RGB2GRAY)
  fEndTime = time.time()-fStartTime
  print "Image converted to grayscale in "+str(fEndTime)+" seconds"
  return (grayImage)
  
def getTransform(inputTri,outputTri,mapMat):
  fStartTime = time.time()
  cv.GetAffineTransform(outputTri,inputTri,mapMat)
  fEndTime = time.time()-fStartTime
  print "Transform matrix computed in "+str(fEndTime)+" seconds"
  return (mapMat)
  
def register(referenceImage,sourceImage,mapMat):
  fStartTime = time.time()
  s = cv.GetSize(referenceImage)
  registeredImage = cv.CreateImage(s,cv.IPL_DEPTH_8U,3)
  cv.WarpAffine(sourceImage,registeredImage,mapMat,cv.CV_WARP_FILL_OUTLIERS,(0,255,0,0))
  fEndTime = time.time()-fStartTime
  print "Registration applied in "+str(fEndTime)+" seconds"
  return (registeredImage)
  
def matchSizes(referenceImage,registeredImage):
  fStartTime = time.time()
  s1 = cv.GetSize(referenceImage)
  print s1
  s2 = cv.GetSize(registeredImage)
  print s2
  if (s1[0]<s2[0])&(s1[1]<s2[1]):
    print "Registered image is larger than reference image, will be downsampled."
  if (s1[0]>s2[0])&(s1[1]>s2[1]):
    print "Referene image is larger than registered image, will be downsampled."
  fEndTime = time.time()-fStartTime
  print "Image sized matched in "+str(fEndTime)+" seconds"
  return()
  
def findFeatures(inputImage):
  fStartTime = time.time()
  s = cv.GetSize(inputImage)
  cornerImage = cv.CreateImage(s,cv.IPL_DEPTH_8U,1)
  cv.Copy(inputImage,cornerImage)
  inputMat = cv.GetMat(inputImage)
  eigImage = cv.CreateMat(s[0],s[1],cv.CV_8UC1)
  tempImage = cv.CreateMat(s[0],s[1],cv.CV_8UC1)
  cornerCount = 3
  qualityLevel = .01
  minDistance = 100
  corners = cv.GoodFeaturesToTrack(inputImage,eigImage,tempImage,cornerCount,qualityLevel,minDistance)
  radius = 10
  color = 0
  for x in corners:
    print "Good feature at "+str(x)
    center = (int(x[0]),int(x[1]))
    cv.Circle(cornerImage,center,radius,color,-1)
  fEndTime = time.time()-fStartTime
  print "Fiducials dynamically detected in "+str(fEndTime)+" seconds"
  return(cornerImage)
  
def verifyFiducial(refPoint,sourcePoint):
  fStartTime = time.time()
  d = cv.Sqrt((refPoint[0]-sourcePoint[0])^2+(refPoint[1]-sourcePoint[1])^2)
  if d<30:
    fEndTime = time.time()-fStartTime
    print "Cannot detect fiducial object in source image, "+str(fEndTime)+" seconds"
    return(False)
  else:
    fEndTime = time.time()-fStartTime
    print "Fiducial verified in "+str(fEndTime)+" seconds"
    return(True)
  
def imageDiff(referenceImage,registeredImage):
  fStartTime = time.time()
  s = cv.GetSize(referenceImage)
  referenceImMat = cv.GetMat(referenceImage)
  registeredImMat = cv.GetMat(registeredImage)
  for i in range(s[1]):
    for j in range(s[0]):
      if cv.Get2D(registeredImMat,i,j)==(0,255,0,0):
        print "pixel at "+str(i)+","+str(j)+" is green"
        cv.Set2D(registeredImMat,i,j,cv.Get2D(referenceImMat,i,j))    
  diffImage = cv.CreateImage(s,cv.IPL_DEPTH_8U,1)
  cv.AddWeighted(referenceImage,1,registeredImage,-1,0,diffImage)
  fEndTime = time.time()-fStartTime
  print "Image diff computed in "+str(fEndTime)+" seconds"
  return (diffImage)
  
def countNonZeros(diffImage):
  fStartTime = time.time()
  nonZeros = cv.CountNonZero(diffImage)
  fEndTime = time.time()-fStartTime
  print "Non-zero pixels counted in "+str(fEndTime)+" seconds"
  return(nonZeros)
  
def trackFeatures(inputImage):
  fStartTime = time.time()
  s = cv.GetSize(inputImage)
  eigImage = cv.CreateImage(s,cv.IPL_DEPTH_8U,1)
  tempImage = cv.CreateImage(s,cv.IPL_DEPTH_8U,1)
  cornerCount = 3
  qualityLevel = .01
  minDistance = 50
  corners = cv.GoodFeaturesToTrack(inputImage,eigImage,tempImage,cornerCount,qualityLevel,minDistance)
  fEndTime = time.time()-fStartTime
  print "Good features tracked in "+str(fEndTime)+" seconds"
  return(corners)
  
def makeFiducialsFromFeatures(inputImage,corners):
	s = cv.GetSize(inputImage)
	f1 = corners[0]
	f2 = corners[1]
	f3 = corners[2]
	f1 = (int(f1[0]),int(f1[1]))
	f2 = (int(f2[0]),int(f2[1]))
	f3 = (int(f3[0]),int(f3[1]))
	maskSize = (20,20)
	mask1 = cv.CreateImage(maskSize,cv.IPL_DEPTH_8U,1)
	mask2 = cv.CreateImage(maskSize,cv.IPL_DEPTH_8U,1)
	mask3 = cv.CreateImage(maskSize,cv.IPL_DEPTH_8U,1)
	for y in range(maskSize[0]):
		for x in range(maskSize[1]):
			cv.Set2D(mask1,y,x,cv.Get2D(inputImage,f1[1]+y,f1[0]+x))
			cv.Set2D(mask2,y,x,cv.Get2D(inputImage,f2[1]+y,f2[0]+x))
			cv.Set2D(mask3,y,x,cv.Get2D(inputImage,f3[1]+y,f3[0]+x))
	return(mask1,mask2,mask3,f1,f2,f3)
	
####################################################################### MAIN Function

referenceImage,sourceImage,grayReferenceImage,graySourceImage,maskFace,maskMagenta,maskA,imName,imDir = initiate(sys.argv)

## Try out GoodFeaturesToTrack
corners = trackFeatures(grayReferenceImage)
print corners
imageWithMarkedFeatures = drawFiducials(grayReferenceImage,corners[0],corners[1],corners[2])
mask1,mask2,mask3,f1,f2,f3 = makeFiducialsFromFeatures(grayReferenceImage,corners)
cv.SaveImage(imDir+"\\"+imName+"GoodFeaturesToTrack.jpg",imageWithMarkedFeatures)
cv.SaveImage(imDir+"\\"+imName+"mask1.jpg",mask1)
cv.SaveImage(imDir+"\\"+imName+"mask2.jpg",mask2)
cv.SaveImage(imDir+"\\"+imName+"mask3.jpg",mask3)

# knownLocFace = (467,365)
# knownLocMagenta = (202,608)
# knownLocA = (163,46)

## Locate fiducials in reference image
maxLocMask1 = matchMask(grayReferenceImage,mask1)
flag1 = verifyFiducial(maxLocMask1,f1)
if not flag1:
	print "Face fiducial not found"
# maxLocMagentaRef = matchMask(grayReferenceImage,maskMagenta)
# flag2 = verifyFiducial(maxLocMagentaRef,knownLocMagenta)
# if not flag2:
	# print "Magenta fiducial not found"
# maxLocARef = matchMask(grayReferenceImage,maskA)
# flag3 = verifyFiducial(maxLocARef,knownLocA)
# if not flag3:
	# print "Circled A fiducial not found"

# resizedSource = resizeToSmallerImage(referenceImage,sourceImage)
# resizedGraySource = convertToGray(resizedSource)

## Locate fiducials in source image
# maxLocFace = matchMask(resizedGraySource,maskFace)
# flag1 = verifyFiducial(maxLocFaceRef,maxLocFace)
# maxLocMagenta = matchMask(resizedGraySource,maskMagenta)
# flag2 = verifyFiducial(maxLocMagentaRef,maxLocMagenta)
# maxLocA = matchMask(resizedGraySource,maskA)
# flag3 = verifyFiducial(maxLocARef,maxLocA)

# print flag1,flag2,flag3
# if not(flag1|flag2|flag3):
  # print "IMAGE COMPARE FAIL - One or more fiducials not detected in source image."
  # endTime = time.time()-startTime
  # print "Executed in "+str(endTime)+" seconds."
  # sys.exit()

# imWithMax = drawFiducials(resizedGraySource,maxLocFace,maxLocMagenta,maxLocA)
# cv.SaveImage(imDir+"\\"+imName+"Max.jpg",imWithMax)

# inputTri = (maxLocARef,maxLocFaceRef,maxLocMagentaRef)
# outputTri = (maxLocA,maxLocFace,maxLocMagenta)
# mapMat = cv.CreateMat(2,3,cv.CV_32F)
# mapMat = getTransform(inputTri,outputTri,mapMat)

# registeredImage = register(referenceImage,resizedSource,mapMat)
# grayRef = convertToGray(referenceImage)
# grayReg = convertToGray(registeredImage)
# diffImage = imageDiff(grayRef,grayReg)
# nonZeros = countNonZeros(diffImage)
# print "The number of non-zero pixels is "+str(nonZeros)

# cv.SaveImage(imDir+"\\"+imName+"REG.jpg",registeredImage)
# cv.SaveImage(imDir+"\\"+imName+"DIFF.jpg",diffImage)

endTime = time.time()-startTime
print "Executed in "+str(endTime)+" seconds."